/*
 * CenterMassBW3.cpp
 *
 *  Created on: May 12, 2015
 *      Author: marchi
 */

#include "CenterMassBW3.h"
bool CenterMassBW3::BeenThere=false;
double CenterMassBW3::Dt=0.0;


std::ifstream & operator>>(std::ifstream & fin, CenterMassBW3 & y){
	y.ReadIt(fin);
	return fin;
}
void CenterMassBW3::ReadIt(ifstream & fin){
	int r;
	size_t mm,nn;
	fin.read(reinterpret_cast<char *> (&time_c), sizeof(time_c));
	if(Dt < 0.0) Dt+=time_c;
	fin.read(reinterpret_cast<char *> (&r), sizeof(r));
	vector<int> rr(r);
	fin.read(reinterpret_cast<char *> (&rr[0]), (sizeof rr[0])*r);
	fin.read(reinterpret_cast<char *> (&nn), sizeof(nn));
	fin.read(reinterpret_cast<char *> (&mm), sizeof(mm));
	fin.read(reinterpret_cast<char *> (&CO), sizeof(Matrix));
	fin.read(reinterpret_cast<char *> (&OC), sizeof(Matrix));

	vector<double> t(r*DIM);
	vector<double> v(r*(DIM+1));
	fin.read(reinterpret_cast<char *> (&t[0]), sizeof(t[0])*r*DIM);
	fin.read(reinterpret_cast<char *> (&v[0]), sizeof(v[0])*r*(DIM+1));
	vector<double> tmR(nn*DIM);
	vector<double> tmL(nn*DIM);
	vector<double> tmX(mm*DIM);
	vector<string> MyLabels(mm);
	fin.read(reinterpret_cast<char *>(&tmR[0]),sizeof(tmR[0])*nn*DIM);
	fin.read(reinterpret_cast<char *>(&tmL[0]),sizeof(tmL[0])*nn*DIM);
	if(!BeenThere) {
		Labels=vector<string>(mm);
		for(auto p=0;p<mm;p++) {
			string labels(5,' ');
			fin.read(&labels[0],sizeof(char)*5);
			labels.erase(remove_if(labels.begin(),labels.end(),::isspace),labels.end());
			Labels[p]=labels;
		}
		X=vvector_d(mm,vector<Dvect>());
		Dt=-time_c;
	}
	fin.read(reinterpret_cast<char *> (&tmX[0]), (sizeof tmX[0])*mm*DIM);

	vector<Dvect> x(mm);
	for(auto n=0;n<mm;n++){
		for(auto o=0;o<DIM;o++)
			x[n][o]=tmX[DIM*n+o];
		X[n].push_back(x[n]);
	}
	BeenThere=true;
}
void CenterMassBW3::Diffusion(ofstream & fout){
	alglib::real_1d_array Ux,Uy,Uz,Cx,Cy,Cz,Sab,Saa,S;
	alglib::ae_int_t Ndim=X[0].size();

	Ux.setlength(Ndim);
	Uy.setlength(Ndim);
	Uz.setlength(Ndim);
	Cx.setlength(2*Ndim-1);
	Cy.setlength(2*Ndim-1);
	Cz.setlength(2*Ndim-1);
	Sab.setlength(Ndim);
	Saa.setlength(Ndim);
	S.setlength(Ndim);

	for(auto it=Labels.begin();it != Labels.end();++it){
		if(Diffs.find(*it) == Diffs.end()){
			Diffs[*it]=vector<double>(Ndim,0.0);
			nDiffs[*it]=0;
		}
	}
	for(auto n=0;n<X.size();n++){
		for(auto o=0;o<X[n].size();o++){
			Ux[o]=X[n][o][XX];
			Uy[o]=X[n][o][YY];
			Uz[o]=X[n][o][ZZ];
		}
		double dsq=0.0;
		for(auto o=0;o<Ndim;o++)
			dsq+=Ux[o]*Ux[o]+Uy[o]*Uy[o]+Uz[o]*Uz[o];


		alglib::corrr1d(Ux,Ndim,Ux,Ndim,Cx);
		alglib::corrr1d(Uy,Ndim,Uy,Ndim,Cy);
		alglib::corrr1d(Uz,Ndim,Uz,Ndim,Cz);
		for(auto o=0;o<Ndim;o++) Sab[o]=Cx[o]+Cy[o]+Cz[o];
		Saa[0]=2.0*dsq;
		for(auto o=1;o<Ndim;o++){
			double r0=Ux[o-1]*Ux[o-1]+Uy[o-1]*Uy[o-1]+Uz[o-1]*Uz[o-1];
			double r1=Ux[Ndim-o]*Ux[Ndim-o]+Uy[Ndim-o]*Uy[Ndim-o]+Uz[Ndim-o]*Uz[Ndim-o];
			Saa[o]=Saa[o-1]-r0-r1;
		}
		for(auto o=0;o<Ndim;o++){
			S[o]=(Saa[o]-2.0*Sab[o])/static_cast<double>(Ndim-o);
		}

		for(auto o=0;o<Ndim;o++) {
			Diffs[Labels[n]][o]+=S[o];
		}
		nDiffs[Labels[n]]++;
	}
	auto nLegend=0;
	fout << "# Grace file " <<endl;
	fout << "# Times are in ns" <<endl;
	fout << "# Distances are in nm^2" <<endl;
	fout << "# " <<endl;
	for(auto it=Diffs.begin();it != Diffs.end();++it){
		long int accu=nDiffs[it->first];
		string lab=it->first;
		for(auto o=0;o<Ndim;o++)
			it->second[o]/=static_cast<double>(accu);
		stringstream legend;
		legend << "@    S";
		legend.width(3);
		legend << std::fixed << std::left << nLegend++ ;
		legend<< " legend   "<< "\"" << it->first << "\"" <<endl;
		fout << legend.str() ;
	}
	nLegend=0;
	for(auto it=Diffs.begin();it != Diffs.end();++it){
		stringstream legend;
		legend << "@target G0.S";
		legend.width(3);
		legend << std::fixed << std::left << nLegend++ << endl;
		legend << "@type xy" <<endl;
		fout << legend.str();
		for(auto o=0;o< it->second.size();o++){
			fout << std::fixed<< std::setprecision(3) << Dt*static_cast<double>(o)/1000.0 ;
			fout << " " ;
			fout << std::fixed << std::setw(10)<< std::setprecision(5)<< it->second[o] <<endl;
		}
		fout << "&"<<endl;
	}

}
void CenterMassBW3::WriteIt(ostream & fout) {
	vector<double> t(cm.size()*DIM);
	vector<double> v;
	for(auto q: Q_t){
		Quaternion qq=q.normalized();
		v.push_back(qq[0]);
		v.push_back(qq[1]);
		v.push_back(qq[2]);
		v.push_back(qq[3]);
	}
	auto n=0;
	for(auto v: cm)
		for(auto q=0;q<DIM;q++)
			t[n++]=v[q];

	int r=cm.size();
	vector<int> rr(r);
	for(auto i=0;i<r;i++) rr[i]=cmm[i].size();

	vector<double> tmR;
	vector<double> tmL;
	vector<double> tmX;


	double time=CenterMass::getTime();
	vector<vector<Dvect> > cmL=cmm;
	vector<vector<Dvect> > cmR=cmm;
	RotateBack(cmR);

	auto po=[](vector<double> & x,vector<vector<Dvect> > & y){
		auto p=0;for(auto & R0: y)
			for(auto & R1: R0)
				for(auto m=0;m<DIM;m++)
					x.push_back(R1[m]); };
	po(tmR,cmR);
	po(tmL,cmL);
	po(tmX,xc);
	auto nn=tmR.size()/DIM;
	auto mm=tmX.size()/DIM;
	fout.write((char *) &time, sizeof time);
	fout.write((char *) &r, sizeof r);
	fout.write((char *) &rr[0], (sizeof rr[0])*r);
	fout.write((char *) &nn, sizeof nn);
	fout.write((char *) &mm, sizeof mm);
	fout.write(reinterpret_cast<const char *> (&CO), sizeof(Matrix));
	fout.write(reinterpret_cast<const char *> (&OC), sizeof(Matrix));
	fout.write((char *) &t[0], (sizeof t[0])*r*DIM);
	fout.write((char *) &v[0], (sizeof v[0])*r*(DIM+1));
	fout.write((char *) &tmR[0], (sizeof tmR[0])*nn*DIM);
	fout.write((char *) &tmL[0], (sizeof tmL[0])*nn*DIM);

	if(!BeenThere) {
		const size_t strDIM=5;
		for(auto o=0;o<mm;o++){
			Labels[o].append(string(strDIM,' '),Labels[o].size()-1,strDIM-Labels[o].size());
			fout.write(Labels[o].c_str(),sizeof(char)*strDIM);
		}
	}
	fout.write(reinterpret_cast<const char *> (&tmX[0]), (sizeof tmX[0])*mm*DIM);
	BeenThere=true;
}


CenterMassBW3::CenterMassBW3() {
}
CenterMassBW3::~CenterMassBW3() {
	// TODO Auto-generated destructor stub
}


string CenterMassBW3::HeavyAtoms1="S C";
string CenterMassBW3::HeavyAtoms2="O S C";

void CenterMassBW3::RotateTranslate(vvector_d & t){
	for(auto n=0;n< t.size();n++){
		auto qq=Q_t[n];
		auto Rot=MatrixfromQ(qq.inverse());
		auto R_cm=cm[n];
		for(auto & pv: t[n]){
			Dvect t0=pv;
			t0=t0-R_cm;
			pv=Rot*t0;
		}
	}
}

void CenterMassBW3::RefCoord(rvec * x, vector<string> & atn, vvector_i & ind, vvector_i & mats){
	if(Labels.empty()) {
		for(auto o=0;o<ind.size();o++)
			for(auto p=0;p<ind[o].size();p++)
				for(auto q=0;q<mats[p].size();q++){
					auto r=mats[p][q];
					if(HeavyAtoms1.find(atn[r][0]) != string::npos)
						Labels.push_back(atn[r]);
				}
	}
	vvector_d t(ind.size());
	for(auto o=0;o<ind.size();o++){
		for(auto p=0;p<ind[o].size();p++){
			for(auto q=0;q<mats[p].size();q++){
				auto r=mats[p][q];
				if(HeavyAtoms1.find(atn[r][0]) != string::npos)
					t[o].push_back(Dvect(x[r]));
			}
		}
	}
	RotateTranslate(t);
	xc=t;
}

void CenterMassBW3::operator ()(vector<Dvect> & v,  vector<vector<Dvect> > & vm, vector<Quaternion> & Q) {
	CenterMass::operator()(v,Q);
	for(auto i=0;i<vm.size();i++){
		try{if(DoneOnce) if(cmm[i].size() != vm[i].size()) throw "The number of molecules in clusters changes from step to step!! ";}
		catch(const char * s){cout << s <<endl;exit(1);}
		cmm[i]=vm[i];
	}
	DoneOnce=true;
}

